#pragma config(Hubs,  S1, HTMotor,  none,     none,     none)
#pragma config(Hubs,  S3, HTServo,  none,     none,     none)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Sensor, S3,     ,               sensorI2CMuxController)
#pragma config(Motor,  mtr_S1_C1_1,     motorRight,    tmotorTetrix, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C1_2,     motorLeft,     tmotorTetrix, openLoop)
#pragma config(Servo,  srvo_S3_C1_1,    hook,                 tServoStandard)
#pragma config(Servo,  srvo_S3_C1_2,    servo2,               tServoNone)
#pragma config(Servo,  srvo_S3_C1_3,    servo3,               tServoNone)
#pragma config(Servo,  srvo_S3_C1_4,    servo4,               tServoNone)
#pragma config(Servo,  srvo_S3_C1_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S3_C1_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "JoystickDriver.c"
task main ()
{

	//servo[hook] = 30;
	//wait1Msec(1000);
	//servo[hook] = 180;
	//wait1Msec(1000);

	while (true)
	{
		getJoystickSettings(joystick);
		// control the motor
		if (joystick.joy1_y1 < -5 || joystick.joy1_y1 > 5)
			motor[motorLeft] = joystick.joy1_y1 / 0.25;
		else
			motor[motorLeft] = 0;

		if (joystick.joy1_y2 < -5 || joystick.joy1_y2 > 5)
			motor[motorRight] = joystick.joy1_y2 / 0.25;
		else
			motor[motorRight] = 0;

		if (joy1Btn(2) == 1)
			servo[hook] = 10;

		if (joy1Btn(4) == 1)
			servo[hook] =180;
	}

}
